import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
from threading import Thread
from log.pylog import log
LOGS_INFO = log.info
class Obstacle():
    def __init__(self):
        self.LIDAR_ERR = 0.05
        self._cmd_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
        self.scan_filter = []
        # self.obstacle()
        
    def get_left_scan(self):
        msg = rospy.wait_for_message("scan", LaserScan)
        self.scan_filter.clear()
        # for i in range(360):
        #     if i <= 190 or i > 170:
        for i in range(269,272):
            if msg.ranges[i] >= self.LIDAR_ERR:
                self.scan_filter.append(msg.ranges[i])
        if  self.scan_filter != []:
            return(min(self.scan_filter))
        return None

    def get_scan(self):
        msg = rospy.wait_for_message("scan", LaserScan)
        self.scan_filter.clear()
        # for i in range(360):
        #     if i <= 190 or i > 170:
        for i in range(269,272):
            if msg.ranges[i] >= self.LIDAR_ERR:
                self.scan_filter.append(msg.ranges[i])
        return min(self.scan_filter)

    def obstacle(self):
        self.twist = Twist()
        while not rospy.is_shutdown():
            self.get_scan()
            rospy.loginfo('distance of the obstacle : %f', min(self.scan_filter))
            if min(self.scan_filter) < 0.5:
                self.twist.linear.x = 0.0
                self.twist.angular.z = 0.0
                self._cmd_pub.publish(self.twist)
                rospy.loginfo('Stop!')

            else:
                self.twist.linear.x = 0.0
                self.twist.angular.z = 0.0
                # rospy.loginfo('distance of the obstacle : %f', min(self.scan_filter))

            self._cmd_pub.publish(self.twist)


def forwrd_p(cmd_vel,move_cmd):
    rate = 50
    r = rospy.Rate(rate)
    while True:
        if move_cmd.linear.x >= -0.01:
            break
        cmd_vel.publish(move_cmd)
        r.sleep()

def adopt_forward(speed=0.30,target_dis=0.40):
    cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
    move_cmd = Twist()
    # 初始设置速度
    # 设置目标距离

    move_cmd.linear.x = speed
    laser = Obstacle()
    # 获取真实距离
    laser_dis = laser.get_scan()
    init_minus_val = laser_dis - target_dis
    forward_thread = Thread(target=forwrd_p,args=[cmd_vel,move_cmd])
    forward_thread.start()
    while laser_dis > target_dis:
        rospy.loginfo('distance of the obstacle : %f', laser_dis)
        laser_dis = laser.get_scan()
        # 速度循环调节
        if speed > 0.05:
            minus_val = laser_dis - target_dis
            speed *= minus_val/init_minus_val 
            move_cmd.linear.x = speed 
        rospy.loginfo("speed:{} ".format(speed))
    move_cmd.linear.x = 0
    cmd_vel.publish(Twist())
    if forward_thread.isAlive():
        forward_thread.join()
    

def get_laser_dis(move_cmd,laser):
    laser_dis = None
    while laser_dis == None:
        empty_times = 0
        laser_dis = laser.get_left_scan()
        empty_times += 1
        if empty_times  >=2:
            move_cmd.linear.x += 0.1
            rospy.loginfo("get laser Empty!")
    return laser_dis
        
def reverse_adopt_forward(speed=0.30,target_dis=0.40):
    cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
    move_cmd = Twist()
    # 初始设置速度
    # 设置目标距离

    move_cmd.linear.x = speed
    laser = Obstacle()
    laser_dis = None
    # 获取真实距离
    laser_dis = get_laser_dis(move_cmd,laser)
    print(laser_dis)
    init_minus_val = target_dis - laser_dis
    forward_thread = Thread(target=forwrd_p,args=[cmd_vel,move_cmd])
    forward_thread.start()
    while laser_dis < target_dis:
        LOGS_INFO('distance of the obstacle : %f', laser_dis)
        laser_dis = get_laser_dis(move_cmd,laser)
        # 速度循环调节
        if speed < -0.05:
            minus_val = target_dis - laser_dis 
            speed *= minus_val/init_minus_val 
            move_cmd.linear.x = speed 
        rospy.loginfo("speed:{} ".format(speed))
    move_cmd.linear.x = 0
    cmd_vel.publish(Twist())
    if forward_thread.isAlive():
        forward_thread.join()

def reverse_adopt_forward_route(point):
    further_points = [0,1,3,4,5,6,9]
    target_dis = 2.5
    if point in further_points:
        target_dis = 2.22
    reverse_adopt_forward(-0.2,target_dis)

def main():
    rospy.init_node('turtlebot_scan')
    try:
        obstacle = Obstacle()
        while 1:
            # dis =  obstacle.get_left_scan()
            dis =  obstacle.get_scan()
            if dis != None:
                rospy.loginfo('distance of the obstacle : %f', dis)
    except rospy.ROSInterruptException:
        pass

if __name__ == '__main__':
    main()
